﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Text.RegularExpressions;
using System.Threading.Tasks;

namespace IndustryWrapper
{
    public class CPoint5D
    {
        public double x, y, z, a, b;
        public CPoint5D clone()
        {
            CPoint5D p = new CPoint5D();
            p.x = x;
            p.y = y;
            p.z = z;
            p.a = a;
            p.b = b;
            return p;
        }
    }
    public class CPoint6D
    {
        public double x, y, z, a, b,c;
        public CPoint6D clone()
        {
            CPoint6D p = new CPoint6D();
            p.x = x;
            p.y = y;
            p.z = z;
            p.a = a;
            p.b = b;
            p.c = c;
            return p;
        }
    }
    /// <summary>
    /// 二维点
    /// </summary>
    public class CPoint2D
    {
        public double x = 0, y = 0;
        public CPoint2D()
        {
            x = 0;
            y = 0;
        }
        public CPoint2D(double x,double y)
        {
            this.x = x;
            this.y = y;
        }
        /// <summary>
        /// 向量角度[0,2pi)
        /// </summary>
        public double angle
        {
            get
            { 
                double rst = 0;
                if (x == 0 && y == 0)
                    return rst;
                if (x == 0)
                {
                    if (y > 0)
                    {
                        rst = Math.PI/2.0;
                    }
                    else
                    {
                        rst = Math.PI*1.5;
                    }
                }
                else if (y == 0)
                {
                    if (x > 0)
                    {
                        rst = 0;
                    }
                    else
                    {
                        rst =  Math.PI;
                    }
                }
                else
                {
                    double value = Math.Abs(y / x);
                    if (x >0 && y > 0)
                    {
                        rst = Math.Atan(value);
                    }
                    else if (x <0 && y > 0)
                    {
                        rst = Math.PI - Math.Atan(value);//出国bug
                    }
                    else if (x < 0 && y < 0)
                    {
                        rst = Math.Atan(value) + Math.PI;
                    }
                    else
                    {
                        rst =2.0*Math.PI - Math.Atan(value);
                    }
                }
                return rst;
            }
        }
        public object clone()
        {
            CPoint2D p = new CPoint2D();
            p.x = this.x;
            p.y = this.y;
            return p;
        }
    }
    /// <summary>
    /// 三位坐标点
    /// </summary>
    public class CPoint3D
    {
        public double x = 0, y = 0, z = 0;
        public CPoint3D clone()
        {
            CPoint3D p = new CPoint3D();
            p.x = x;
            p.y = y;
            p.z = z;
            return p;
        }
        public CPoint3D()
        {
            x = 0; y = 0; z = 0;
        }
        public CPoint3D(double tx, double ty, double tz)
        {
            x = tx; y = ty; z = tz;
        }

        public double getDist(CPoint3D pt)
        {
            return Math.Sqrt((pt.x - x) * (pt.x - x) + (pt.y - y) * (pt.y - y) + (pt.z - z) * (pt.z - z));
        }
        /// <summary>
        /// 对点进行顶点旋转多少度
        /// </summary>
        /// <param name="center">旋转中心点</param>
        /// <param name="angle">角度 单位是度  不是弧度 </param>
        public void rotate(CPoint3D center, double angle)
        {
            double angleHude = angle * Math.PI / 180;/*角度变成弧度*/
            double rstx = (x - center.x) * Math.Cos(angleHude) - (y - center.y) * Math.Sin(angleHude) + center.x;
            double rsty = (x - center.x) * Math.Sin(angleHude) + (y - center.y) * Math.Cos(angleHude) + center.y;
            x = rstx;
            y = rsty;
        }
        /// <summary>
        /// 对点进行平移
        /// </summary>
        /// <param name="distX"></param>
        /// <param name="distY"></param>
        public void translate(double distX, double distY, double distZ)
        {
            x += distX;
            y += distY;
            z += distZ;
        }
    }
    /// <summary>
    /// 所有的计算工具
    /// </summary>
    public class CMathTool
    {
        /// <summary>
        /// 产品坐标系转换到绝对坐标系
        /// 当前项目只考虑了:
        /// 1 A轴是一直为0度
        /// 2 AB旋转中心是重叠的.
        /// 3 pt的Y是XYZAB机构中的Z;
        /// 如果从剖面来看,产品必须是逆时针示教.
        /// </summary>
        /// <param name="pt">产品表面上的一个点</param>
        /// <param name="angle">位置点对应的角度[0,2PI)</param>
        /// <param name="offset">刀具偏移</param>
        /// <param name="tpt">目标点</param>
        public static void tran_xyz_xyzab(CPoint3D pt, double angle, double offset, ref CPoint5D tpt)
        {
            double toolneedangle = angle - Math.PI / 2.0;//逆时针示教的时候,几个象限都考虑了。
            double detax = Math.Cos(toolneedangle) * offset;
            double detaz = Math.Sin(toolneedangle) * offset;
            //由于是反算回去,所以XZ方向都是相反的
            tpt.x = pt.x + detax;
            tpt.y = pt.y;
            tpt.z = pt.z + detaz;
            tpt.a = 0;
            //直线角度转换成B轴的角度-注意这里必须是逆时针示教
            //tpt.b = angle*180.0/Math.PI - 180.0;//TODO:考虑各种象限
            tpt.b = angle * 180.0 / Math.PI - 90.0 + 180.0 - 270.0;
        }
        /// <summary>
        /// xyzab坐标转换成工件坐标的XYZ
        /// 当前项目只考虑了:
        /// 1 A轴为0的情况,B轴在运动来计算.
        /// 2 AB的轴旋转中心是重叠的.
        /// </summary>
        /// <param name="pt">直接的坐标</param>
        /// <param name="tpt">计算出的产品坐标,相对于机床坐标点</param>
        /// <param name="lenthB">工具长度  B轴旋转中心到表面的长度</param>
        /// <param name="machMark">机床的夹具中心点在XYZ坐标系中的位置</param>
        /// <returns></returns>
        public static bool tran_xyzab_xyz(CPoint5D pt, ref CPoint3D tpt,double lenthB,double[] machMark)
        {
            double hudu = (pt.b + 270.0)/180.0*Math.PI;//以垂直方向 及270度为0
            double detaZ = lenthB * Math.Sin(hudu);
            double detaX = lenthB * Math.Cos(hudu);
            tpt.x = pt.x + detaX - machMark[0];
            tpt.y = pt.y - machMark[1];
            tpt.z = pt.z + detaZ - machMark[2];
            return true;
        }
        /// <summary>
        /// 获取某一位的值
        /// </summary>
        /// <param name="value">输入值</param>
        /// <param name="bit">0-x的位数</param>
        /// <returns></returns>
        public static bool getBit(int value, int bit)
        {
            return (value & (1 << bit)) != 0;
        }
        /// <summary>
        /// 获取某一位的值
        /// </summary>
        /// <param name="value">输入值</param>
        /// <param name="bit">0-x的位数</param>
        /// <returns></returns>
        public static bool getBit(UInt32 value, int bit)
        {
            return (value & (1 << bit)) != 0;
        }
        /// <summary>
        /// P1指向P2的的一条线段 离开P1的指令距离上的一个点
        /// </summary>
        /// <param name="p1">直线第一个点</param>
        /// <param name="p2">直线第二个点</param>
        /// <param name="tLenth">离开第1个点的长度</param>
        /// <param name="rstPt">结果</param>
        /// <returns></returns>
        public static bool line_cal_point(CPoint2D p1, CPoint2D p2, double tLenth, ref CPoint2D rstPt)
        {
            if (tLenth < 0)
                return false;
            if (tLenth == 0)
            {
                rstPt.x = p1.x;
                rstPt.y = p1.y;
                return true;
            }
            CPoint2D v = new CPoint2D(p2.x - p1.x, p2.y - p1.y);
            if (v.x == 0 && v.y == 0)
                return false;
            double angle = v.angle;
            double detaX = 0, detaY = 0;

            detaX = Math.Cos(angle) * tLenth;
            detaY = Math.Sin(angle) * tLenth;
            rstPt.x = p1.x + detaX;
            rstPt.y = p1.y + detaY;
            return true;
        }
        /// <summary>
        /// 求一个直线的角
        /// x1y1的点指向x2y2
        /// </summary>
        /// <param name="x1"></param>
        /// <param name="y1"></param>
        /// <param name="x2"></param>
        /// <param name="y2"></param>
        /// <returns>[0,2PI)</returns>
        public static bool line_angle(double x1, double y1, double x2, double y2,ref double angle)
        {
            if (Math.Sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)) < 0.001)
                return false;
            CPoint2D p = new CPoint2D((x2 - x1), (y2 - y1));
            angle = p.angle;
            return true;
        }
        /// <summary>
        /// 计算圆心
        /// </summary>
        /// <param name="ptOnArc">圆弧上的一个点</param>
        /// <param name="angle">这个点指向圆心的角度[0,2PI)</param>
        /// <param name="radius">半径</param>
        public static bool arc_cal_center(CPoint2D ptOnArc, double angle, double radius, ref CPoint2D cpt)
        {
            if (radius <= 0)
                return false;
            if (angle < 0 || angle >= 2.0 * Math.PI)
                return false;
            if (Math.Abs(angle) < 0.00001)//0du
            {
                cpt.x = ptOnArc.x + radius;
                cpt.y = ptOnArc.y;
            }
            else if (Math.Abs(angle - Math.PI / 2.0) < 0.00001)//90
            {
                cpt.x = ptOnArc.x;
                cpt.y = ptOnArc.y + radius;
            }
            else if (Math.Abs(angle - Math.PI) < 0.00001)//180
            {
                cpt.x = ptOnArc.x - radius;
                cpt.y = ptOnArc.y;
            }
            else if (Math.Abs(angle - Math.PI / 2.0) < 0.00001)//270
            {
                cpt.x = ptOnArc.x;
                cpt.y = ptOnArc.y - radius;
            }
            else
            {
                double detax = Math.Cos(angle) * radius;
                double detay = Math.Sin(angle) * radius;
                cpt.x = ptOnArc.x + detax;
                cpt.y = ptOnArc.y + detay;
            }
            return true;
        }
        /// <summary>
        /// 获得一个圆上面离开指定点多少角度的另外个点
        /// </summary>
        /// <param name="c">圆</param>
        /// <param name="pt">指定的点</param>
        /// <param name="angle">运动角度 >0代表逆时针  <0顺时针  [0,PI)</param>
        /// <param name="rstpt">结果</param>
        /// <returns></returns>
        public static bool arc_point_angle(CPoint2D center,double radius, CPoint2D pt, double angle, ref CPoint2D rstpt)
        {
            //获取圆
            double tmpx = 0, tmpy = 0;
            double startAngle = 0;
            double endAngle = 0;
            //平移
            tmpx = pt.x - center.x;
            tmpy = pt.y - center.y;
            if (tmpx > 0 && tmpy >= 0)
            {
                startAngle = Math.Atan(tmpy / tmpx);
            }
            else if (tmpx < 0 && tmpy >= 0)
            {
                startAngle = Math.PI - Math.Atan(Math.Abs(tmpy / tmpx));
            }
            else if (tmpx < 0 && tmpy <= 0)
            {
                startAngle = Math.PI + Math.Atan(Math.Abs(tmpy / tmpx));
            }
            else if (tmpx > 0 && tmpy <= 0)
            {
                startAngle = 2 * Math.PI - Math.Atan(Math.Abs(tmpy / tmpx));
            }
            else if (tmpx == 0)
            {
                if (tmpy > 0)
                {
                    startAngle = Math.PI / 2.0;
                }
                else
                {
                    startAngle = 1.5 * Math.PI;
                }
            }
            endAngle = startAngle + angle;
            //计算XY
            rstpt.x = radius * Math.Cos(endAngle);
            rstpt.y = radius * Math.Sin(endAngle);
            rstpt.x += center.x;
            rstpt.y += center.y;
            return true;
        }
        /// <summary>
        /// 获取圆弧/圆上到某个点一定距离的点
        /// </summary>
        /// <param name="rstPt">获取到的点</param>
        /// <param name="cpt">圆弧或者圆的中心</param>
        /// <param name="ept">制定的圆弧上的某个点</param>
        /// <param name="radius">圆弧半径</param>
        /// <param name="L">到制定点的距离</param>
        /// <param name="direct">方向 true= 逆时针旋转</param>
        public static bool arc_point_dist(ref CPoint2D rstPt, CPoint2D cpt, CPoint2D ept, double radius, double L, bool direct)
        {
            //获取圆
            double tmpx = 0, tmpy = 0;
            double eptAngle = 0;
            //平移
            tmpx = ept.x - cpt.x;
            tmpy = ept.y - cpt.y;
            double addAngle = L / radius;//增量角度
            if (tmpx > 0 && tmpy >= 0)
            {
                eptAngle = Math.Atan(tmpy / tmpx);
            }
            else if (tmpx < 0 && tmpy >= 0)
            {
                eptAngle = Math.PI - Math.Atan(Math.Abs(tmpy / tmpx));
            }
            else if (tmpx < 0 && tmpy <= 0)
            {
                eptAngle = Math.PI + Math.Atan(Math.Abs(tmpy / tmpx));
            }
            else if (tmpx > 0 && tmpy <= 0)
            {
                eptAngle = 2 * Math.PI - Math.Atan(Math.Abs(tmpy / tmpx));
            }
            else if (tmpx == 0)
            {
                if (tmpy > 0)
                {
                    eptAngle = Math.PI / 2.0;
                }
                else
                {
                    eptAngle = 1.5 * Math.PI;
                }
            }
            //旋转叫
            if (direct)//逆时针
            {
                eptAngle = eptAngle - addAngle;
            }
            else
            {
                eptAngle = eptAngle + addAngle;
            }
            //计算XY
            rstPt.x = radius * Math.Cos(eptAngle);
            rstPt.y = radius * Math.Sin(eptAngle);
            rstPt.x += cpt.x;
            rstPt.y += cpt.y;
            return true;
        }
    }
    /// <summary>
    /// 所有指令都要在这里创建一次
    /// 主要用于初始化静态构造体
    /// 否则没有使用的时候是不会被调用的
    /// </summary>
    public class CCmdRegeister
    {
        static CG01 l= new CG01();
        static CG02 arc = new CG02();
        static CG00 g0 = new CG00();
        static CG03 g03 = new CG03();
        static CCmdCircle c = new CCmdCircle();
    }
    
}
